/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.ortn.wildbots.subsystems;

import edu.ortn.wildbots.RobotMap;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 *
 * @author Stephen
 */
public class CameraMount extends Subsystem {
    public Servo tilt;
    public Servo pan;
    
    private double speed = 3;
    private double vertSpeed = 2;
    
    private static final int MAX_ANGLE = 240;
    private static final int MIN_ANGLE = 0;
    
    public static final int CENTERED_X = 108;

    public CameraMount() {
        tilt = new Servo(RobotMap.TILT_SERVO);
        pan = new Servo(RobotMap.PAN_SERVO);
    }
    
    public void initDefaultCommand() {
        
    }
    
    public void setSpeed(double speed) {
        this.speed = speed;
    }
    
    public void panLeft(){
        //System.out.println("Pan Angle: "+pan.getAngle());
        if(pan.getAngle()-speed>=MIN_ANGLE&&pan.getAngle()-speed<=MAX_ANGLE){
            pan.setAngle(pan.getAngle()-speed);
        }

    }
    
    public void panRight(){
        //System.out.println("Pan Angle: "+pan.getAngle());
        if(pan.getAngle()+speed>=MIN_ANGLE&&pan.getAngle()+speed<=MAX_ANGLE){
            pan.setAngle(pan.getAngle()+speed);
        }
    }
    
    public void tiltUp(){
        //System.out.println("tAngle: "+tilt.getAngle());
        //System.out.println("Speed: " + speed);
        if(tilt.getAngle()+speed>=MIN_ANGLE&&tilt.getAngle()+speed<=MAX_ANGLE){
            //System.out.println("IN RANGE");
            tilt.setAngle(tilt.getAngle()+vertSpeed);
        }
    }
    
    public void tiltDown(){
        //System.out.println("tAngle: "+tilt.getAngle());
        if(tilt.getAngle()-speed>=MIN_ANGLE&&tilt.getAngle()-speed<=MAX_ANGLE){
                tilt.setAngle(tilt.getAngle()-vertSpeed); 
        }
    }
    public void setCenter(){
        pan.setAngle(CENTERED_X);
        tilt.setAngle(0);
    }
    
    public String[] getData() {
        String[] data = new String[3];
        data[0] = "Pan: " + pan.getAngle();
        data[1] = "Tilt: " + tilt.getAngle();
        data[2] = "CamSpeed: " + speed;
        return data;
    }
    
    public void move(double x, double y){
        tilt.setAngle(y);
        pan.setAngle(CENTERED_X + x); 
    }
}
